#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <pthread.h>
#include <semaphore.h>
#include "robot.h"

#define BAUD_RATE B57600

static int serial;

int init_serial(char *port)
{
	struct termios config;
	unsigned char full_mode[] = {128, 132};
	
	serial = open(port, O_RDWR | O_NOCTTY /*| O_NDELAY*/);
	if(serial < 0) {
		return -1;
	}
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	//config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUD_RATE) < 0 || cfsetospeed(&config, BAUD_RATE) < 0) {
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) {
		return -3;
	}
	 
	write(serial, full_mode, 2);

}

//comsumer thread of sorts... looks for info in the buffer and writes it to serial when it finds it.
void run_serial()
{
	buffer_item item;
	int i = 0;
	
	//keeps track of the command that is coming into the buffer
	//only necessary if we have to accumulate an entire command before passing it through serial
	//buffer_item command[5];

	while(1)
	{
		//wait until we have an item in the buffer
		sem_wait(&full);
		
		//lock the mutex
		pthread_mutex_lock(&mutex);
		
		//clear the buffer
		//for(i = 0; i < 5; i++)
		//{
		//	command[i] = NULL;
		//}
		
		//i = 0;
		//while(command[4] == NULL)
		//{
			//remove an item from the buffer
			remove_item(&item))
		//	command[i] = item;
		//	i++;
		//}
		
		//write(serial, command, 5);
		write(serial, item, 1);
		
		//release the lock and post the empty semaphore
		sem_post(&empty);
		pthread_mutex_unlock(&mutex);
	}
}

int remove_item(buffer_item *item)
{
	int success = -1;
	
	//remove item from the buffer at position count
	if(count > 0)
	{
		success = 0; //we are successful
		*item = buffer[remove_loc];
		buffer[remove_loc] = NULL;
		count--;
		
		//set next remove location
		remove_loc++;
		if(remove_loc >= BUFFER_SIZE)
		{
			remove_loc = 0;
		}
	}
	
	//return the success condition, 0 = success, -1 = error
	return success;
}

int insert_item(buffer_item item)
{
	int success = -1;
	
	//add item to the buffer
	if(count < BUFFER_SIZE)
	{
		success = 0; //we are successful
		buffer[insert_loc] = item;
		count++;
		
		//set next insert location
		insert_loc++;
		if(insert_loc >= BUFFER_SIZE)
		{
			insert_loc = 0;
		}
	}
	
	//return the success condition, 0 = success, -1 = error
	return success;
}

void run_keyboard()
{
	unsigned int command[5];
	
	
	while(1)
	{
		//printf("> ");
		scanf("%c", &c);
		if(c == 'q') {
			robot_close();
			exit(0);
		}
		else if(c == 'f') {
			command[] = {145, 0, 100, 0, 100};
		}
		else if(c == 'b') {
			command[] = {145, 255, 156, 255, 156};
		}
		else if(c == 'l') {
			command[] = {145, 0, 100, 255, 156};
		}
		else if(c == 'r') {
			command[] = {145, 255, 156, 0, 100};
		}
		else if(c == 's') {
			command[] = {145, 0, 0, 0, 0};
		}
		//allow us to also check the bump sensors on demand
		else if(c == 't') {
			robot_bumpers(&bumper, &stamp);
			printf("bumbers: ");
			if(bumper < 0 || bumper > 3) {
				printf("not valid data\n");
			}
			printf("left=%s   ", ((bumper & 2) == 2) ? "yes" : "no");
			printf("right=%s   ", ((bumper & 1) == 1) ? "yes" : "no");
			printf("time=%d\n", (int) difftime(stamp, start_time));
		}
		
		////<<------------------------------------------------------------------------------------QUESTION
		//perhaps we could spin this off as a separate thread that would then end its done adding the command?
		//This would make the buffer actually useful....
		move_robot(command[]);
	}
}

void move_robot(unsigned char command[])
{
	int i;
	//only this function can add until it's done adding
	pthread_mutex_lock(&adding);
	
	//add each command to the buffer individually
	for(i = 0; i < 5; i++)
	{
		pthread_mutex_lock(mutex);
		insert_item(command[i]);
		pthread_mutex_unlock(mutex);
	}
	
	pthread_mutex_unlock(&adding);
}

void robot_bumpers(unsigned char *data, time_t *timestamp)
{
	unsigned char bump[] = {142, 7};
	write(serial, bump, 2);
	//usleep(100000);
	read(serial, data, 1);
	(*data) &= 3;
	*timestamp = time(NULL);
}

int robot_close()
{
	return close(serial);
}
